//#include "stdafx.h"
#include "utilities.h"
using namespace std;
using namespace cv;

double euclideanDist(Point p1, Point p2)
{
	Point diff = p1 - p2;
	double val = diff.x*diff.x + diff.y*diff.y;
	val =  sqrt(val);
	return val;
}

double getAngle(Point p,Point p1, Point p2)
{
	double diff1 = sqrt(double((p.x - p1.x)*(p.x - p1.x) + (p.y - p1.y)*(p.y - p1.y)));
	double diff2 = sqrt(double((p.x - p2.x)*(p.x - p2.x) + (p.y - p2.y)*(p.y - p2.y)));
	double res = atan2((diff1),(diff2)) * 180.0 / CV_PI;
	return res;
}


int FindBigestClosedContour(vector<vector<Point> > &contours, double areaOfFrame)
{
	double max=-1;
	int maxPosition=-1;
	double tempArea;
	//LOGD("xxx utilities::FindBigestClosedContour contours.size() %d",contours.size());
	//LOGD("xxx utilities::FindBigestClosedContour areaOfFrame %f",areaOfFrame);
	for(int i = 0; i < contours.size(); i++)
	{
		tempArea = contourArea((contours[i]));
		if (tempArea > max && tempArea > areaOfFrame/sizeRatioToFrame)
		{
			max = tempArea;
			maxPosition = i;
		}
	}
//LOGD("xxx utilities::FindBigestClosedContour max area %f",max);
	return maxPosition;
}

double CalcAngleOfSegment(vector<vector<Point> > &contours, int index, int maxPosition)
{
	double angle = 0;
	int prevPixel = index - offset;
	if (prevPixel < 0)
	{
		prevPixel += contours[maxPosition].size();
	}
	int nextPixel = index + offset;
	if (nextPixel >= contours[maxPosition].size())
	{
		nextPixel = nextPixel - contours[maxPosition].size();
	}
	angle=abs(getAngle(contours[maxPosition][index],contours[maxPosition][prevPixel],contours[maxPosition][nextPixel]));
	while (angle > 180)
	{
		angle = angle - 180;
	}
	return angle;
}

bool CoordinatesMakeSense(Point topLeft, Point bottomLeft, Point topRight,Point bottomRight)
{
//LOGD("xxx utilities::CoordinatesMakeSense overlay %d",(topLeft != topRight && topLeft != bottomLeft && bottomRight != topRight && bottomRight != bottomLeft));

//LOGD("xxx utilities::CoordinatesMakeSense top/down %f",(abs(euclideanDist(topLeft,topRight)/euclideanDist(bottomLeft,bottomRight) - 1) ));

//LOGD("xxx utilities::CoordinatesMakeSense left/right %f",(abs(euclideanDist(topLeft,bottomLeft)/euclideanDist(topRight,bottomRight) - 1) ));
	return (topLeft != topRight && topLeft != bottomLeft && bottomRight != topRight && bottomRight != bottomLeft &&//points are not overlaping 
	abs(euclideanDist(topLeft,topRight)/euclideanDist(bottomLeft,bottomRight) - 1) < reasonableFactor&&//top and down segment ratio is reasonable
	abs(euclideanDist(topLeft,bottomLeft)/euclideanDist(topRight,bottomRight) - 1) < reasonableFactor);//left and right segment ratio is reasonable
}

vector<vector<Point> > RetrieveContours(Mat &mSrc, int optionEQ, int optionMask)
{
	//LOGD("xxx RetrieveContours");
	clock_t begin = clock();
	vector<vector<Point> > contours;
	vector<Vec4i> hierarchy;	

	resize(mSrc, mSrc,  Size(), RESIZE_IMAGE, RESIZE_IMAGE);

	Mat temp;
	Mat BGR,HSV;
	if (optionMask == InnerMask || optionMask == OuterMask)
	{
		//LOGD("xxx RetrieveContours before convert to HSV");
		cvtColor(mSrc, HSV, CV_BGR2HSV);
		//LOGD("xxx RetrieveContours after convert to HSV");
		vector<Mat> hsv_planes;
		split( HSV, hsv_planes );
		temp = hsv_planes[1].clone();
		CV_Assert(!temp.empty());
	}
	else
	{
		//LOGD("xxx RetrieveContours before convert to gray");
		cvtColor(mSrc, temp, CV_BGR2GRAY);
		//LOGD("xxx RetrieveContours after convert to gray");
	}
	
	if (optionEQ == DoEQ)
	{
		equalizeHist( temp, temp );
	}
	calcMask(temp,temp, optionMask);
	//LOGD("xxx RetrieveContours after mask");
	medianBlur(temp, temp, 5);

	clock_t end = clock();
	double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	LOGD("ttt preproccesing time %f",elapsed_secs);
	//LOGD("xxx RetrieveContours after blur");

	double thresh_high =-1;
	double thresh_low;
	string name1,name2;
	if (optionMask == NoMask && optionEQ == NoEQ)
	{
		name1 = "canny";
		name2 = "before canny";
		thresh_high = CANN_THRESH_high_reg;
		thresh_low = CANN_THRESH_low_reg;
	}
	if (optionMask == NoMask && optionEQ == DoEQ)
	{
		name1 = "canny eq";
		name2 = "before canny eq";
		thresh_high = CANN_THRESH_high_eq;
		thresh_low = CANN_THRESH_low_eq;
	}
	if (optionMask == InnerMask && optionEQ == NoEQ)
	{
		name1 = "canny InnerMask";
		name2 = "before canny InnerMask";
		thresh_high = CANN_THRESH_high_mask_inner;
		thresh_low = CANN_THRESH_low_mask_inner;
	}
	if (optionMask == OuterMask && optionEQ == NoEQ)
	{
		name1 = "canny OuterMask";
		name2 = "before canny OuterMask";
		thresh_high = CANN_THRESH_high_mask_outer;
		thresh_low = CANN_THRESH_low_mask_outer;
	}

	if (thresh_high == -1)
	{
	// shouldn't be here, it's wrong configuration
		name1 = "canny OuterMask";
		name2 = "before canny OuterMask";
		thresh_high = CANN_THRESH_high_mask_outer;
		thresh_low = CANN_THRESH_low_mask_outer;
	}
	if (VISO_TEST)
	{
		imshow(name2, temp);
	}
	begin = clock();
	Canny(temp, temp, thresh_low, thresh_high, 5);
	if (VISO_TEST)
	{
		imshow(name1, temp);
	}
	end = clock();
	elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	LOGD("ttt Canny time %f",elapsed_secs);

	begin = clock();
	findContours(temp, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
	end = clock();
	elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	LOGD("ttt findContours time %f",elapsed_secs);
	//LOGD("xxx RetrieveContours end");
	return contours;
}
struct sorter
{
	Point host;
    sorter(Point host) : host(host) { }
    bool operator()(Point first, Point second){
      return euclideanDist (host,first) < euclideanDist (host,second);
    }
};
//gets angle current point 
//update the point as an extreme point in case it should be
void UpdateRectangleCorners(double angle,Point ref, std::list<Point> &topLeft, std::list<Point> &bottomLeft, std::list<Point> &topRight,std::list<Point> &bottomRight, int height, int width)
{
	Point reftopleft	=	Point(0,0);
	Point refbottomLeft	=	Point(0,height);
	Point refbottomRight	=	Point(width,height);
	Point reftopRight	=	Point(width,0);

	if (angle >= 90 - angleLimit && angle <= angleLimit + 90)
	{
		topLeft.push_back (ref);
		topLeft.sort(sorter(reftopleft));
		if (topLeft.size() > ListSize)
		{
			topLeft.pop_back();
		}

		topRight.push_back (ref);
		topRight.sort(sorter(reftopRight));
		if (topRight.size() > ListSize)
		{
			topRight.pop_back();
		}

		bottomLeft.push_back (ref);
		bottomLeft.sort(sorter(refbottomLeft));
		if (bottomLeft.size() > ListSize)
		{
			bottomLeft.pop_back();
		}

		bottomRight.push_back (ref);
		bottomRight.sort(sorter(refbottomRight));
		if (bottomRight.size() > ListSize)
		{
			bottomRight.pop_back();
		}
	}
}


void UpdateRectangleCorners(double angle,Point ref, Point &topLeft, Point &bottomLeft, Point &topRight,Point &bottomRight, int height, int width)
{
	Point reftopleft	=	Point(0,0);
	Point refbottomLeft	=	Point(0,height);
	Point refbottomRight	=	Point(width,height);
	Point reftopRight	=	Point(width,0);

	if (angle >= 90 - angleLimit && angle <= angleLimit + 90)
	{
		if (euclideanDist(topLeft, reftopleft) >= euclideanDist(ref, reftopleft))
		{
			topLeft = ref;
		}
		if (euclideanDist(topRight, reftopRight) >= euclideanDist(ref, reftopRight) )
		{
			topRight = ref;
		}
		if (euclideanDist(bottomLeft, refbottomLeft) >= euclideanDist(ref, refbottomLeft) )
		{
			bottomLeft = ref;
		}
		if (euclideanDist(bottomRight, refbottomRight) >= euclideanDist(ref, refbottomRight) )
		{
			bottomRight = ref;
		}
	}
}
//gets image and configuration 
//returns rectangle corners
int* GetRectCorners(Mat &mSrc, int optionEQ,int optionMask)
{
	//LOGD("xxx GetRectCorners");
	clock_t begin = clock();
	vector<vector<Point> > contours = RetrieveContours(mSrc, optionEQ, optionMask);
	clock_t end = clock();
	double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	LOGD("ttt RetrieveContours time %f",elapsed_secs);

	begin = clock();
	int maxPosition = FindBigestClosedContour(contours,mSrc.size().height*mSrc.size().width);
	end = clock();
	elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	LOGD("ttt FindBigestClosedContour time %f",elapsed_secs);
	if (maxPosition > -1 && contours.size() > maxPosition)
	{	
		int *res;
		begin = clock();
		res =  match_points(contours , maxPosition,mSrc.size().height,mSrc.size().width);
		end = clock();
		elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
		LOGD("ttt match_points time %f",elapsed_secs);

		if (res[0] != -1)
		{
			int *ret=new int[8];			
			for (int i = 0; i<8; i++)
			{
				ret[i] = res[i];
			}

			//cout<<"Father ";
			for (int i = 0; i<8; i++)
			{
				cout<<ret[i]<<" ";
			}
			cout<<endl;
			return ret;
		}
	}
	int *ret=new int[1];	
	ret[0] = -1;
	return ret;
}

Point meanNoOutliers(std::list<Point> listOfPoints)
{
	int i=0;
	double u_x=0;
	double u_y=0;
	double std_x=0;
	double std_y=0;

	std::list<Point>::iterator iter = listOfPoints.begin();
	while (iter != listOfPoints.end())
	{
		u_x+=iter->x;
		u_y+=iter->y;

		std_x += iter->x*iter->x;
		std_y += iter->y*iter->y;
		i++;
		iter++;
	}
	u_x=u_x/i;
	u_y=u_y/i;
	std_x=std_x/i;
	std_y=std_y/i;

	std_x=std_x - u_x*u_x;
	std_x = sqrt(std_x);
	std_y=std_y - u_y*u_y;
	std_y = sqrt(std_y);
	Point res(0,0);
	i=0;
	iter = listOfPoints.begin();
	while (iter != listOfPoints.end())
	{
		/*LOGD("ttt abs(iter->x - u_x) %f",abs(iter->x - u_x));
		LOGD("ttt std_x %f",std_x);
		LOGD("ttt abs(iter->y - u_y) %f",abs(iter->y - u_y));
		LOGD("ttt std_y %f",std_y);*/
		if (abs(iter->x - u_x)<=std_x && abs(iter->y - u_y)<=std_y )
		{
			res+=*iter;
			i++;
		}
		iter++;
	}
	if (i>0)
	{
		res=Point(res.x/i,res.y/i);
	}

	return res;
}

void DanDiag(std::list<Point> listOfPoints1,std::list<Point> listOfPoints2, Point &p1,Point &p2)
{
	double max=-1;
	std::list<Point>::iterator iter1 = listOfPoints1.begin();
	std::list<Point>::iterator iter2 = listOfPoints2.begin();
	std::list<Point>::iterator temp = listOfPoints2.begin();
	while (iter1 != listOfPoints1.end())
	{
		iter2 = temp;
		while (iter2 != listOfPoints2.end())
		{
			if (euclideanDist(*iter1,*iter2) >= max)
			{
				p1=*iter1;
				p2=*iter2;
			}
			iter2++;
		}
		iter1++;
		temp++;
	}
}
//gets a contour
//returns the four points that are potentially the rect corners
int* match_points(vector<vector<Point> > contours , int maxPosition, int height, int width)
{
	std::list<Point> topLeft;
	std::list<Point> bottomLeft;
	std::list<Point> topRight;
	std::list<Point> bottomRight;
		
	Point PxMin = contours[maxPosition][0];
	Point PxMax = contours[maxPosition][0];
	Point PyMin = contours[maxPosition][0];
	Point PyMax = contours[maxPosition][0];

	for (int i =0 ; i<contours[maxPosition].size();i++)
	{
		double angle = CalcAngleOfSegment(contours, i,maxPosition);
		UpdateRectangleCorners(angle,contours[maxPosition][i],topLeft,bottomLeft,topRight,bottomRight,height,width);
		Point temp = contours[maxPosition][i];
		if (PxMin.x <= temp.x)
		{
			PxMin = temp;
		}

		if (PyMin.y <= temp.y)
		{
			PyMin = temp;
		}
	
		if (PxMax.x >= temp.x)
		{
			PxMax = temp;
		}

		if (PyMax.y >= temp.y)
		{
			PyMax = temp;
		}

	}

	Point topLeft_t		=	contours[maxPosition][0];
	Point bottomLeft_t	=	contours[maxPosition][0];
	Point topRight_t	=	contours[maxPosition][0];
	Point bottomRight_t	=	contours[maxPosition][0];
	
	UpdateRectangleCorners(90,PxMin,topLeft_t,bottomLeft_t,topRight_t,bottomRight_t,height,width);
	UpdateRectangleCorners(90,PxMax,topLeft_t,bottomLeft_t,topRight_t,bottomRight_t,height,width);
	UpdateRectangleCorners(90,PyMin,topLeft_t,bottomLeft_t,topRight_t,bottomRight_t,height,width);
	UpdateRectangleCorners(90,PyMax,topLeft_t,bottomLeft_t,topRight_t,bottomRight_t,height,width);

	if (CoordinatesMakeSense(topLeft_t,bottomLeft_t,topRight_t,bottomRight_t))
	{
		int *res=new int[8];
		//Edges.push_back (topLeft);
		res[0] = (int)(topLeft_t.x / RESIZE_IMAGE);
		res[1] = (int)(topLeft_t.y / RESIZE_IMAGE);
		//Edges.push_back (topRight);
		res[2] = (int)(topRight_t.x / RESIZE_IMAGE) ;
		res[3] = (int)(topRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomRight);
		res[4] = (int)(bottomRight_t.x / RESIZE_IMAGE);
		res[5] = (int)(bottomRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomLeft);
		res[6] = (int)(bottomLeft_t.x / RESIZE_IMAGE);
		res[7] = (int)(bottomLeft_t.y / RESIZE_IMAGE);
		cout<<"reg";
		return res;
	}

	DanDiag(topLeft,bottomRight,topLeft_t,bottomRight_t);
	DanDiag(bottomLeft,topRight,bottomLeft_t,topRight_t);
	if (CoordinatesMakeSense(topLeft_t,bottomLeft_t,topRight_t,bottomRight_t))
	{
		int *res=new int[8];
		//Edges.push_back (topLeft);
		res[0] = (int)(topLeft_t.x / RESIZE_IMAGE);
		res[1] = (int)(topLeft_t.y / RESIZE_IMAGE);
		//Edges.push_back (topRight);
		res[2] = (int)(topRight_t.x / RESIZE_IMAGE) ;
		res[3] = (int)(topRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomRight);
		res[4] = (int)(bottomRight_t.x / RESIZE_IMAGE);
		res[5] = (int)(bottomRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomLeft);
		res[6] = (int)(bottomLeft_t.x / RESIZE_IMAGE);
		res[7] = (int)(bottomLeft_t.y / RESIZE_IMAGE);
		cout<<"dan";
		return res;
	}
	
	topLeft_t =  meanNoOutliers(topLeft);
	bottomLeft_t =  meanNoOutliers(bottomLeft);
	topRight_t =  meanNoOutliers(topRight);
	bottomRight_t =  meanNoOutliers(bottomRight);
	if (CoordinatesMakeSense(topLeft_t,bottomLeft_t,topRight_t,bottomRight_t))
	{
		int *res=new int[8];
		//Edges.push_back (topLeft);
		res[0] = (int)(topLeft_t.x / RESIZE_IMAGE);
		res[1] = (int)(topLeft_t.y / RESIZE_IMAGE);
		//Edges.push_back (topRight);
		res[2] = (int)(topRight_t.x / RESIZE_IMAGE) ;
		res[3] = (int)(topRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomRight);
		res[4] = (int)(bottomRight_t.x / RESIZE_IMAGE);
		res[5] = (int)(bottomRight_t.y / RESIZE_IMAGE);
		//Edges.push_back (bottomLeft);
		res[6] = (int)(bottomLeft_t.x / RESIZE_IMAGE);
		res[7] = (int)(bottomLeft_t.y / RESIZE_IMAGE);
		cout<<"mean";
		return res;
	}

	int *res = new int[1];
	res[0] = -1;
	return res;
}

void calcMask(Mat &mSrc,Mat &result, int optionMask)
{
	if (optionMask == NoMask)
	{
		Mat * out = new Mat(mSrc.size().height, mSrc.size().width, CV_8UC1);
		* out = mSrc;
		result = * out;
		return;
	}
	int histSize = 256;
	float rangeSat[] = { 0, 255 } ;
	const float* histRangeSat = { rangeSat };
	bool uniform = true; bool accumulate = false;
	Mat hist; 
	Mat tempImage;
	if (optionMask == InnerMask)
	{
		accumulate = false;
		cv::Rect myROI(mSrc.size().width / 2 -INNER_SIZE ,mSrc.size().height / 2 -INNER_SIZE,INNER_SIZE,INNER_SIZE);
		tempImage = mSrc(myROI);
		calcHist( &tempImage, 1, 0, Mat(), hist, 1, &histSize, &histRangeSat, uniform, accumulate );
	}
	else
	{
		accumulate = true;
		cv::Rect myROI_up(0,0,mSrc.size().width,FRAME_SIZE);
		cv::Rect myROI_left(0,FRAME_SIZE,FRAME_SIZE,mSrc.size().height - FRAME_SIZE);
		cv::Rect myROI_down(FRAME_SIZE,mSrc.size().height -FRAME_SIZE ,mSrc.size().width - FRAME_SIZE ,FRAME_SIZE);
		cv::Rect myROI_right( mSrc.size().width - FRAME_SIZE,FRAME_SIZE ,FRAME_SIZE,mSrc.size().height - FRAME_SIZE);
		tempImage = mSrc(myROI_up);
		calcHist( &tempImage, 1, 0, Mat(), hist, 1, &histSize, &histRangeSat, uniform, accumulate );
		tempImage = mSrc(myROI_left);
		calcHist( &tempImage, 1, 0, Mat(), hist, 1, &histSize, &histRangeSat, uniform, accumulate );
		tempImage = mSrc(myROI_down);
		calcHist( &tempImage, 1, 0, Mat(), hist, 1, &histSize, &histRangeSat, uniform, accumulate );
		tempImage = mSrc(myROI_right);
		calcHist( &tempImage, 1, 0, Mat(), hist, 1, &histSize, &histRangeSat, uniform, accumulate );
	}
	float max = -1;
	int indSat =-1;
	for( int i = 0; i < histSize; i++ )
	{
		float temp = hist.at<float>(i);
		if (temp >= max)
		{
			max = temp;
			indSat =i;
		}
	}
	Mat dst1 =0*mSrc;
	Mat dst2 =0*mSrc;
	threshold(mSrc, dst1, indSat + Hue_range, 1 ,THRESH_BINARY_INV);
	threshold(mSrc, dst2, indSat - Hue_range, 1 ,THRESH_BINARY);
	Mat * out = new Mat(mSrc.size().height, mSrc.size().width, CV_8UC1);
	*out = ((dst1.mul(dst2)))*255;
	result = *out;
}
